/*
 * Part of the program sensors
 * Print the data from input 0 (GP1)
 */
#include <xtask.h>
#include <rrcon.h>
#include <log.h>
#include <printfstring.h>

#include "sensors_read.h"
#include "sensors_print.h"
#include "gyro_manager.h"

#define PERIOD 5 //Hz

namespace sensors_test {
    using namespace kthrobot;

    SensorsPrinter::SensorsPrinter(SensorsDataQueue *data_queue, AnglesQueue *angle_queue)
        : XTask("SensorsPrinter"),data_queue_(data_queue),gyro_manager_(angle_queue) {
            SetPeriodHz(PERIOD);
        }

    SensorsPrinter::~SensorsPrinter() {
        Join();
    }

    void SensorsPrinter::TaskCode() {
        while(!MustStop()) {
            ADCRawData data;
            // Printing raw values
            if (data_queue_->Deq(&data, (double) 1/PERIOD)) {
                string output_raw = "Raw data: | ";
                string output_cm = "Distance: | ";
                for(int i=0; i<8; i++) {
                    output_raw += PrintFString("%i: [%04d] | ",i,data.channel[i]);
                    output_cm += PrintFString("%i: %03d cm | ",i,SensorsReader::ConvertToCm(data.channel[i],i));
                }
                Log::Write(output_raw);
                Log::Write(output_cm);
            }
            // Printing angle move
            Log::Write("Angle since start: %i", gyro_manager_.get_angle());
            WaitPeriod();
        }
    }
};
